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1.  INTRODUCTION 


The  various  aspects  of  Strapdown  Inertial  Navigation  Systems  have  been  subjects  of 
investigation  in  the  U.S.A.  for  the  past  15  years  or  so.  More  recently,  interest  has  been  shown 
from  other  countries,  having  been  stimulated  by  the  demonstration  of  suitable  sensors  and 
computing  hardware.  Problems  related  to  computation,  which  is  fundamental  to  a  strapdown 
system,  have  received  the  most  attention.  Reference  I  is  a  literature  survey  of  strapdown  tech¬ 
nology,  with  some  300  references.  Reference  2  contains  a  bibliography  of  approximately  230 
references.  Reference  3  is  a  review  of  the  fundamentals  of  strapdown  I.N.  systems. 

Papers  dealing  with  strapdown  inertial  navigation  systems  are  often  rather  fragmentary  in 
that  they  deal  with  certain  aspects  in  isolation,  and  may  be  obscure  to  those  not  familiar  with 
the  complete  subject.  In  this  and  following  papers  an  attempt  is  being  made  to  document  a 
complete  and  viable  system  in  a  form  which  may  be  followed  by  those  less  familiar  with  the 
subject.  To  this  end,  a  survey  of  all  the  alternative  methods  has  not  been  included,  except  for 
a  few  examples  for  comparison  purposes. 


1.1  The  Concept  of  Strapdown  I.N.S. 

An  inertial  measurement  unit  consisting  of  a  minimum  of  three  gyroscopes  and  three 
accelerometers  is  mounted  directly  or  perhaps  with  vibration  isolators  to  the  body  of  the  vehicle. 
Associated  with  this  is  a  computer  which  processes  the  sensors’  outputs  and  performs  the 
navigation  calculations.  The  gyroscopes  are  usually  arranged  with  their  sensitive  axes  nominally 
mutually  perpendicular.  The  accelerometers  are  similarly  arranged. 

At  a  particular  time,  the  en-route  vehicle  has  a  certain  position,  velocity,  and  attitude 
relative  to  a  specified  reference.  After  a  short  time,  the  vehicle  has  moved  to  a  new  position, 
and  the  velocity  and  attitude  have  changed.  During  this  period,  the  sensor  outputs  are  observed. 
The  gyroscope  outputs  are  used  to  calculate  the  change  in  attitude  over  the  period.  The  updated 
attitude  of  the  vehicle  is  then  calculated.  With  allowances  for  the  effects  of  gravity,  the  accelero¬ 
meter  outputs  are  used  to  calculate  increments  of  velocity  over  the  period.  Because  the  accelero¬ 
meters  are  fixed  to  the  vehicle,  the  direction  of  these  increments  changes  as  the  vehicle  attitude 
changes.  However,  because  the  attitude  of  the  vehicle  is  known,  these  velocity  increments  can  be 
expressed  relative  to  the  reference,  and  so  the  change  in  position  can  be  calculated. 

This  process  has  been  going  on  continuously  since  the  commencement  of  the  flight,  so 
assuming  position,  velocity,  and  attitude  were  known  at  the  start,  navigation  has  been  achieved. 
Before  commencement  of  navigation,  a  “levelling  and  alignment"  sequence  is  performed.  This 
is  the  process  of  acquiring  the  initial  conditions,  and  is  often  referred  to  simply  as  “alignment". 

1.2  Computation  in  Strapdown  I.N.S. 

The  computations  performed  by  a  strapdown  navigator  may  be  regarded  as  comprising 
two  major  parts:  propagation  of  the  attitude  reference,  and  solution  of  the  navigation  equation. 
The  former  uses  the  gyro  outputs  to  calculate  the  attitude  of  the  body  coordinate  frame  with 
respect  to  a  reference  coordinate  frame.  The  latter  uses  this  relationship  to  transform  the  coordi¬ 
nates  of  vectors  (which  may  include  accelerometer  outputs,  velocities,  gravity  effects,  Earth 
rotation  and  curvature  effects,  etc.)  between  frames  and  hence  to  calculate  velocity  and  position 
of  the  body. 

Other  calculations  performed  in  the  navigator  include  sensor  compensation  (e.g.  correction 
of  gyro  outputs  for  known  drifts,  etc.)  and,  before  the  commencement  of  navigation,  levelling 
and  alignment  of  the  system’s  internal  references.  Alignment  may  be  carried  out  with  the  I.N.S. 
nominally  at  rest,  such  as  when  an  aircraft  is  parked  on  the  ground,  or  when  the  system  is  in 
motion,  if  another  source  of  navigation  information  is  available.  The  latter  is  known  as  transfer 
alignment. 
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1.3  Content  of  this  Paper 

In  this  paper  an  algorithm  for  solution  of  the  attitude  and  navigation  equations  is  presented. 
The  algorithm  and  the  theory  behind  it  are  discussed.  Where  practical,  the  mathematics  have 
been  confined  to  appendices,  for  reference  as  required. 

This  analysis  is  thought  to  be  unique  in  its  presentation  in  that  the  process  has  been  split 
into  sequentially  performed  modules  each  of  which  may  be  analysed  in  isolation.  This  approach 
allows  clearer  insight  into  the  workings  of  the  process  and  considerably  facilitates  modification 
of  the  algorithm  to  provide  greater  or  lesser  accuracy  (in  return  for  greater  or  lesser  computer 
loading)  as  required. 

In  particular,  the  attitude  updating  part  of  the  algorithm  has  been  split  into  two  parts — 
the  solution  for  a  “rotation  vector’',  and  the  update  of  the  quaternions.  The  navigation  part 
of  the  algorithm  uses  a  split  frame  technique  whereby  body  related  quantities  are  evaluated  in 
body  axis  coordinates,  and  navigation  frame  related  quantities  are  evaluated  in  navigation  axis 
coordinates.  Additionally,  the  algorithm  is  partitioned  into  three  sections  which  are  performed 
at  different  rates  according  to  the  application. 

2.  UPDATING  THE  ATTITUDE  REFERENCE 

All  strapdown  system  mechanisations  must  maintain  a  relationship  between  the  body  frame 
ind  some  reference  frame.  The  accuracy  of  this  relationship  is  critical  to  the  successful  operation 
of  the  system. 

This  relationship  may  be  the  actual  coordinate  transformation  matrix  between  the  two 
frames:  that  is  the  direction  cosine  matrix.  Alternatively  it  may  be  some  parameter  set  from 
which  a  transformation  may  be  obtained  later.  The  latter  approach,  using  quaternions  (see 
Appendix  2),  is  the  preferred  technique,  although  direct  updating  of  the  direction  cosine  matrix 
is  sometimes  reported. 

Updating  is  conventionally  achieved  by  the  solution  of  the  differential  equation  governing 
the  parameter : 

for  a  direction  cosine  matrix  the  equation  is 

c  =  cn 


and  for  quaternions  the  equation  is 

Q  =  $Q  •  o> 

(see  Appendix  1  for  notation). 

These  equations  are  derived  in  Appendix  3.  The  method  of  solution  is  usually  by  either  3rd 
order  Taylor  Series  or  4th  order  Runge-Kutta  (see  Appendix  4).  Higher  order  methods  seem 
to  be  neither  necessary  nor  practical  in  these  real-time  applications. 

The  analysis  presented  here  uses  what  will  be  referred  to  as  the  Rotation  Vector  method. 

This  concept  has  not  received  much  attention  in  the  literature.  It  was  used  by  Bortz  (Ref.  4) 
in  a  proposal  for  a  hybrid  system.  In  practice,  as  here,  its  application  gives  an  end  result  very 
similar  to  the  Taylor  Series  of  equivalent  order.  The  concept  is  further  developed  here  because 
it  is  considered  that  it  provides  a  most  useful  insight  into  the  workings  of  the  mathematical 
process.  This  facilitates  any  modification  of  the  resultant  algorithm  to  provide  greater  or  lesser 
accuracy  as  required. 

Finite  rotations  (such  as  occur  in  a  gyro  sampling  period)  are  non-commutative.  This 
means  that  the  actual  net  rotation  of  the  vehicle  (which  is  required)  is  not  equal  to  the  integral 
of  angular  rate  (the  gyro  output),  unless  the  rotation  of  the  vehicle  is  about  a  fixed  axis  during 
the  sample  period.  In  a  real  system  the  axis  is  not  usually  fixed,  so  a  fast  rate  of  sampling  and 
updating  must  be  used.  For  a  given  level  of  system  accuracy,  the  use  of  the  higher  order  algorithms 
mentioned  above  is  usually  cost-effective  in  use  of  computer  time — the  cost  of  increased  com¬ 
plexity  is  more  than  saved  by  the  gain  from  the  lower  iteration  rate.  In  both  the  3rd  and  4th 
order  methods  mentioned  above  (and  in  the  rotation  vector  method),  the  assumption  is  made 
that,  during  the  iteration  period,  the  gyro  outputs  follow  a  square  law  and  may  therefore  be 
approximated  by  a  second  order  polynominal.  This  requires  two  gyro  samples  per  iteration, 
and  affords  a  substantial  correction  for  non-commutativity  effects. 
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2.1  Rotation  Vector  Updating  Concept 

In  this  method,  the  gyro  outputs  are  corrected  for  non-commutativity  effects  by  calculating 
an  equivalent  fixed  axis  rotation  vector,  which  is  then  used  to  update  the  quaternion. 

2.1.1  Calculation  of  Rotation  Vector  from  Gyroscope  Outputs 

For  a  finite  rotation  about  a  fixed  axis,  a  “rotation  vector”  may  be  defined,  its  direction 
being  along  the  axis  of  rotation,  and  its  length  the  rotation  magnitude.  In  Appendix  2  a  quaternion 
is  shown  as  Q  —  C,  8  S:  here,  0  is  the  rotation  vector.  The  relationship  between  rotation  vector 
and  angular  velocity  is  derived  in  Appendix  5: 

0  =  <i>  +  i<0  x  u>)  +  AB  x  (0  x  u>)  where 

0  o2 

and  Bq?  =  0.0 

Euler's  Theorem  states  that,  for  any  finite  rotation,  there  is  an  equivalent  fixed  axis  rotation. 
The  rotation  vector  approach  to  the  non-commutativity  problem  is  to  use  the  gyro  outputs  to 
solve  the  rotation  vector  equation,  obtaining  the  equivalent  fixed  axis  rotation  over  the  period, 
and  to  use  this  rotation  for  updating  the  attitude  quaternion.  For  this  case,  where  two  gyro 
samples  are  taken  per  interval,  and  the  triple  vector  product  (which  is  very  small)  is  neglected, 
the  numerical  solution  for  the  rotation  vector  0  is  given  by 

0  =  +  *2  4*  x  S2) 

where  5i  and  82  are  the  incremental  gyro  sa  nples,  taken  at  the  mid-point  and  end-point  of  the 
interval.  This  equation  is  derived  in  Appendix  6. 


2.1.2  Quaternion  Update  Using  the  Rotation  Vector 

The  “equivalent  rotation”  is  about  a  fixed  axis;  if  the  rotation  vector  0  is  expressed  as  a 
rotation  quaternion  0,  then  the  updated  quaternion  is  given  by  the  standard  rules  of  quaternion 
multiplication: 

Q  (/  +  St)  =  Q(t)  +  d  where  0  —  C,  0  5  as  in  Appendix  2 

i.e.  C  =  cos  (i^o),  S  =  (l/0o)  sin  (J0O) 
and  0O2  =  0.® 

For  a  given  value  of  0,  the  above  result  is  exact,  subject  to  calculation  of  C  and  S.  These 
can  be  obtained  from  the  series  expansions  for  sine  and  cosine.  The  third  order  expansion  has 
C  =  1  -  W  and  5  =  i  - 

For  fixed  axis  rotation,  Wilcox  (Ref.  5)  showed  how  modified  second  order  expansions  of 
C  and  S  (C  =  1  —  -^o2  and  S  =  0  *  5)  could  give  drift  performance  slightly  superior  to  that 
of  a  third  order  expansion.  This  simplification  is  achieved  at  the  expense  of  increased  scale  errors. 
There  is  little  to  choose  between  this  third  order  and  the  modified  second  order  methods  inaccuracy 
for  fixed  axis  rotation.  In  computer  loading,  the  third  order  requires  an  extra  4  multiplications 
and  1  addition  per  iteration,  with  occasional  normalisation;  the  modified  second  order  requires 
more  frequent  normalisation,  which  involves  8  multiplications  and  4  additions.  These  figures 
do  not  include  multiplication  or  division  by  2.  On  balance,  the  modified  second  order  method 
has  been  found  preferable. 

In  this  mechanisation,  the  equivalent  rotation  vector  is  computed  from  the  gyro  outputs, 
and  the  rotation  update  quaternion  is  obtained  from  this  using  the  modified  second  order 
expansion  for  C  and  5.  This  may  be  considered  as  a  modified  third  order  method :  in  Appendix  4, 
it  is  shown  how  the  rotation  vector  calculation,  together  with  the  third  order  expansion  of 
C  and  5,  is  numerically  almost  equivalent  to  the  third  order  Taylor  Series  solution  of  the  quater¬ 
nion  differential  equation. 


2.2  Modification  of  the  Attitude  Algorithm 

The  Rotation  Vector  Concept  gives  a  useful  insight  into  the  workings  of  the  mathematical 
process,  by  splitting  the  non-commutativity  correction  procedures  from  the  quaternion  update 
procedures. 

Consider  the  equation 

8  =  61  +  82  -}-  j(8i  x  82) 

the  quantity  (81  +  82)  is  of  course  the  sum  of  the  gyro  outputs  over  the  whole  period;  ^81  x  82) 
is  the  non-commutativity  correction.  In  an  application  requiring  less  accuracy,  one  sample  8  per 
iteration  may  be  taken:  then  8  =  8  and  fixed  axis  rotation  is  assumed  (i.e.  81  x  82  —  0). 

Alternatively,  more  than  two  gyro  samples  per  iteration  may  be  taken.  For  example,  if 
three  samples  81,  82,  83  are  taken,  the  gyro  outputs  are  assumed  cubic,  and  the  solution  of  the 
rotation  vector  equation  has  the  form : 

8  =  81  -f  82  +  83  +  cross  product  terms. 

Similarly,  the  quaternion  update  accuracy  may  be  varied  by  taking  fewer  or  more  terms 
in  the  expansions  for  C  and  S. 

2.3  Impact  on  Sensor  Compensation 

A  considerable  amount  of  computer  time  may  be  taken  by  sensor  compensation — allowing 
for  known  biases  and  acceleration  sensitivities,  etc.  It  may  appear  that  taking  several  gyro  samples 
per  iteration  would  multiply  this  time.  However,  the  rotation  vector  solution  is  of  the  form 

8  =  sum  term  4-  cross  product  term. 

The  cross-product  term  is  much  smaller  than  the  sum  term,  so  for  most  applications  it 
should  be  possible  to  compensate  only  the  sum  of  the  gyro  outputs  at  the  end  of  the  period, 
and  calculate  the  cross-product  term  from  uncorrected  intermediate  outputs. 

3.  SOLUTION  OF  THE  NAVIGATION  EQUATION 

The  function  of  the  navigation  algorithm  is  to  accept  the  accelerometer  outputs  and  the 
attitude  parameters,  and  to  calculate  position  and  velocity  of  the  vehicle. 

Accelerometers  respond  to  specific  force,  which  is  the  difference  between  inertial  and 
gravitational  (“mass  attraction”)  acceleration.  An  allowance  must  therefore  be  made  for  gravi¬ 
tation.  In  practice,  it  is  usual  to  calculate  the  value  of  “gravity”,  which  is  defined  as  the  resultant 
of  gravitational  and  centrifugal  acceleration  due  to  Earth's  rotation.  The  Coriolis  effect,  which 
arises  from  the  measurement  of  velocities  relative  to  rotating  axes,  must  also  be  allowed  for. 

In  order  to  measure  vector  quantities,  a  coordinate  frame  must  be  specified.  Many  coordinate 
frames  are  used  in  inertial  navigation  analysis  (Ref.  6),  but  for  present  purposes,  only  two  are 
necessary:  see  Figure  1. 

The  aircraft  Body  frame  is  defined  as  the  orthogonal  set  having  the  Roll  axis  pointing 
forward,  the  Pitch  axis  pointing  out  the  starboard  side,  and  the  Yaw  axis  pointing  “down”  relative 
to  the  aircraft.  The  origin  of  the  body  frame  is  at  the  aircraft  centre  of  mass,  not  coincident  with 
the  I.N.S. 

The  Geographic  frame  has  its  origin  at  the  system’s  location  and  its  axes  aligned  with  the 
local  North,  East,  and  Down  directions.  Down  is  defined  as  normal  to  the  Earth's  reference 
ellipsoid.  In  this  discussion,  the  geographic  frame  is  the  navigation  frame. 

For  any  terrestrial  inertial  naviration  system  analysis,  the  concept  of  an  inertial  frame  is 
also  required.  This  frame  has  its  origin  at  the  mass  centre  of  the  Earth,  and  is  non-rotating 
relative  to  the  stars.  For  present  purposes,  it  is  not  necessary  to  specify  the  directions  of  the  axes 
of  the  inertial  frame. 

The  set  of  sensor  axes  may  be  arranged  in  any  attitude  relative  to  the  body  frame,  although 
conceptually  it  is  useful  to  consider  them  as  coincident  with  the  body  axes.  (Whatever  their 
orientation,  a  nominally  constant  transformation  will  give  their  outputs  relative  to  the  body 
axes.)  For  purposes  of  this  discussion,  the  sensor  and  body  axes  will  be  assumed  coincident 
except  where  mentioned  otherwise. 


Conventionally,  the  navigation  algorithm  takes  the  attitude  reference  and  calculates  the 
coordinate  transformation  matrix  (if  the  reference  is  not  already  the  direction  cosine  matrix). 
It  uses  this  to  transform  the  accelerometer  outputs  from  body  to  geographic  (or  whatever  set 
of  axes  are  being  used  for  navigation)  axes  coordinates.  Present  position  and  velocity  are  used 
to  calculate  the  gravity  and  coriolis  effects.  These  are  added  to  the  accelerometer  outputs  in 
the  appropriate  coordinates,  and  the  resulting  quantities  are  integrated  to  get  velocity  and  position. 
In  a  digital  system  the  accelerometers  are  usually  arranged  to  act  as  acceleration-integrating 
sensors,  so  the  outputs  are  obtained  as  velocity  increments,  integrated  along  body  axes. 

Vehicle  attitude  and  transformation  matrix  are  likely  to  be  changing  rapidly,  therefore  the 
coordinate  transformations  must  be  performed  at  a  fast  rate.  This  imposes  a  considerable  burden 
on  the  computer. 

The  navigation  equation  for  strapdown  inertial  navigation  is  derived  in  Appendix  7:  for 
any  reference  frame  K\ 

=  F  +  8  -  (20«  +  aa) V* 

for  example,  if  AT  is  a  “geographic”  frame  <7,  and  the  quantities  are  expressed  in  G  frame  coordi¬ 
nates,  we  get 

IAV£]c  =  jy  *  C<j  [F)«  dt  +  jy 8'  {[g]c  -  (2 Sl%  +  Qfc)  IVE]°}  dt . 

In  a  “conventional”  mechanisation  this  equation  is  solved  at  a  fast  rate.  The  g  and  ft  terms 
may  not  have  to  be  evaluated  at  the  fast  rate,  but  JC£  [F]8  dt  does. 

In  the  equation,  C%  is  the  body  to  geographic  coordinate  transformation  matrix,  which  is 
varying  rapidly  as  the  aircraft  attitude  changes.  The  output  of  an  integrating  accelerometer  is 
J[F]8  dt.  The  solution  of  J C%  [F]8  dt  has  not  been  widely  discussed  in  the  literature,  and  is  usually 
solved  by  rectangular  or,  preferably,  trapezoidal  integration.  Higher  orders  have  been  recom¬ 
mended:  e.g.  Levinson  (Ref.  7)  considers  that  a  third  order  solution  is  required.  Such  a  solution 
fits  a  second  order  polynominal  to  both  ft  and  F,  and  requires  considerable  computer  time. 

The  aircraft  attitude  rate,  of  up  to  several  hundred  degrees  per  second,  may  be  far  greater 
than  the  navigation  frame  rotation  rate,  which  is  unlikely  to  exceed  one  degree  per  minute. 
Changes  in  gravity,  and  effects  of  Earth  rotation  and  curvature  are  functions  of  the  position 
of  the  navigation  axes  relative  to  Earth:  although  they  change  slowly,  their  effects  on  each  of 
the  system  accelerometers  change  at  the  body  rate. 

The  split  frame  mechanisation  of  the  navigation  equation  takes  advantage  of  the  different 
frame  rates,  by  performing  body-axes-related  calculations  at  a  fast  rate  in  body  coordinates, 
and  navigation-axes-related  calculations  at  slower  rates  in  navigation  axes  coordinates.  This 
leads  to  considerable  savings  in  computer  time. 


3.1  The  Split-Frame  Mechanisation 

This  concept  is  not  new,  but  it  has  not  had  much  attention  in  the  literature.  It  was  mentioned 
by  Wilcox  (Ref.  5),  but  it  did  not  appear  to  attract  further  study  until  a  mathematical  analysis 
was  reported  by  Bar-Itzhak  (Ref.  8).  Wray  and  Flynn  (Ref.  9),  in  a  comparison  of  various 
solutions  of  the  navigation  equation,  concluded  that  it  was  the  most  efficient  in  use  of  computer 
time. 

The  basis  of  the  split  frame  mechanisation  is  the  fact  that  changes  in  position  or  velocity 
caused  by  specific  force  and  changes  in  position  or  velocity  caused  by  gravitation  may  be  calcu¬ 
lated  separately.  The  actual  change  is  the  sum  of  these.  The  effects  of  specific  force  are  evaluated 
at  a  fast  rate  in  body  axes  coordinates.  The  effects  of  gravitation  are  evaluated  at  a  slower  rate 
in  navigation  axes  coordinates.  Coordinates  of  the  incremental  velocity  are  transformed  from 
body  to  navigation  axes  at  the  slower  rate,  leading  to  a  significant  saving  in  computer  loading. 

3.1.1  Body  Axes  Calculations  (Fast  Rate) 

In  this  section,  the  accelerometer  outputs  and  the  rotation  vector  are  used  to  calculate 
increments  in  velocity  caused  by  specific  force.  This  is  performed  at  a  fast  rate  in  body  axes. 
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The  velocity  of  the  body,  with  respect  to  an  inertial  frame,  caused  by  specific  force,  is  given 
by  the  equation 

This  is  derived  in  Appendix  8.  This  equation  is  solved  numerically  at  a  fast  rate,  using  the 
accelerometer  output  and  the  rotation  vector  calculated  for  the  attitude  update.  The  solution 
of  this  equation  is  derived  in  Appendix  9 :  for  present  purposes  the  assumption  is  made  of  constant 
angular  velocity  and  specific  force  during  the  calculation  interval.  This  has  been  found  to  give 
an  adequate  trade  off  between  computer  loading  and  accuracy.  The  resulting  algorithm  is 

-  [v/f]*  +  [Al*  -  [ej*  X  {[V/f]*  +  mB} 
where  [A]B  =  J[F ]B  dt  —  the  accelerometer  output,  and  [QIB]B  is  the  rotation  vector. 

In  Appendix  10  the  relationship  between  V/F  and  V  EF  is  discussed  and  it  is  shown  that 
subject  to  restraints  on  the  frequency  of  the  navigation  frame  calculations,  it  can  be  assumed  that 

[V/f]*=[V£F]*. 

The  [V/F]B  are  calculated  and  accumulated  at  the  fast  rate  until  the  next  navigation  frame 
.^lculation  is  to  take  place.  The  assumption  is  then  made  that  tV£F]B  =  [V/F]*,  and  \EF  is 
transformed  into  navigation  coordinates.  The  initial  condition  for  fast  rate  calculations  is  then 
applied  (V/F  =  R/F  =  0)  and  a  new  series  begins. 


3.1.2  Navigation  Axes  Calculations  (Intermediate  Rate) 

During  the  intermediate  rate  calculation  cycle,  the  attitude  quaternion  is  updated,  to  account 
for  rotation  of  the  navigation  axes  since  the  last  intermediate  cycle,  normalised,  and  then  used 
to  transform  the  coordinates  of  \EF  from  body  to  navigation  axes.  This  is  added  to  the  pre¬ 
viously  calculated  [V£G]*,  giving  the  net  change  in  velocity,  relative  to  Earth,  over  the  interval. 

The  total  velocity  relative  to  Earth,  and  the  change  in  position  since  the  last  slow  cycle, 
are  then  updated.  Using  these  values,  the  navigation  frame  rotation  and  the  [V£C]*  are  calcu¬ 
lated  for  use  in  the  next  intermediate  cycle. 


3.1.3  Navigation  Frame  Rotation 

The  attitude  quaternion  required  for  velocity  coordinate  transformation  relates  the  body 
axes  to  navigation  axes.  However,  during  the  fast  rate  cycles,  the  quaternion  is  being  updated 
with  rotations  of  the  body  relative  to  an  inertial  frame.  (The  gyroscope  outputs  are  relative  to 
inertial  space).  It  is  therefore  necessary  to  allow  for  the  rotation  of  the  navigation  frame  relative 
to  the  inertial  frame,  caused  by  Earth  rotation  and  aircraft  movement  over  the  curved  surface 
of  the  Earth.  Appendix  1 1  shows  how  a  quaternion  multiplication  is  used  to  account  for  this 
rotation.  The  quaternion  represents  the  rotation,  over  the  intermediate  cycle,  of  the  navigation 
frame  relative  to  the  inertial  frame,  and  is  expressed  in  navigation  frame  coordinates. 


3.1.4  Velocity  Transformation 

The  \Ef  coordinates  are  transformed  from  body  to  navigation  axes  using  the  quaternion. 
However,  if  body/navigation  attitude  information  is  required  (e.g.  for  flight  control,  bomb 
aiming,  etc.),  it  is  usually  more  economical  to  calculate  the  direction  cosine  matrix,  and  use 
that  for  the  transformation.  These  procedures  are  well  known,  but  are  listed  for  reference  in 
Appendix  12. 


3.1.5  Calculation  of  Velocity  Change  dne  to  Gravitation  Effects 

The  velocity  of  the  body,  relative  to  Earth,  caused  by  the  effects  of  gravitation,  is  given  by 
the  equation 

[v£cr  -  [«r  -  (2n& + qz*)  [vfCr 
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This  is  derived  in  Appendix  8.  It  can  be  seen  that  this  equation  is  of  a  similar  form  to  the  \JF 
equation :  it  is  solved  numerically  using  the  same  procedure,  the  solution  being 

[V£cf  -  [g]*/,  -  (2Q£  +  U&,)  (IVE]S  +  MNh)  //• 

In  this  mechanisation,  only  the  vertical  component  of  g  is  considered. 

3.1.6  Vertical  Channel  Damping 

The  vertical  channel  of  a  pure  inertial  system  has  an  exponential  instability  with  a  time 
constant  near  the  earth  of  about  9*5  minutes.  This  arises  because  the  value  of  g  is  computed 
on  the  basis  of  the  calculated  altitude.  See  Appendix  14. 

It  is  common  practice  to  stabilise  the  vertical  channel  by  an  external  altitude  reference, 
usually  the  barometric  altimeter.  For  optimal  mixing  a  Kalman  filter  is  used  (e.g.  Ref.  10), 
although  in  cases  where  the  ultimate  in  accuracy  is  not  required,  or  where  computer  capacity 
is  limited,  such  as  in  the  present  algorithm,  a  fixed  gains  system  is  used.  A  typical  third  order 
mechanisation  is  employed,  taken  from  Reference  11.  The  equations  for  this  are  given  in 
Appendix  14,  and  the  triple  pole  time  constant  of  100  seconds  is  retained. 

3.1.7  Position  and  Velocity  Integration 

Having  obtained  [AV£]*  =  [V££]*  -f-  [V£C]*  the  change  in  position  is  calculated: 

[Axr«-[Axr + {(v£r + «dv£f} 

and  the  updated  velocity: 

[v£r-tv£r+tAv£r. 

In  this  mechanisation,  the  [AX]N  are  themselves  changes  in  position  since  the  last  slow  rate  cycle 

3.2  Slow  Rate  Calculations 

During  the  slow  rate  cycle,  updates  of  vehicle  position,  in  latitude  and  longitude,  and  also 
of  the  sine  and  cosine  of  latitude  are  made.  In  this  mechanisation,  the  sine  and  cosine  terms 
are  updated  as  shown  in  Appendix  13.  New  values  of  Earth  radii  and  gravity  are  calculated 
each  slow  cycle:  the  equations  used  are  given  in  Appendix  13. 


4.  SUMMARY  OF  ALGORITHM 

Gyroscopes  are  sampled  twice  per  iteration  period,  accelerometers  once.  The  attitude 
reference  is  maintained  as  a  quaternion.  Gyroscope  outputs  are  used  to  calculate  an  equivalent 
(over  the  period)  rotation  vector,  which  is  used  to  update  (to  third  order)  the  attitude  quaternion. 

A  split  frame  method  is  used  for  solution  of  the  navigation  equation :  body  related  quantities 
are  evaluated  in  body  axes  coordinates,  and  navigation  frame  related  quantities  are  evaluated 
in  navigation  axes  coordinates. 

The  algorithm  is  partitioned  into  3  rates:  fast,  intermediate,  and  slow.  Body  axes  calcu¬ 
lations  are  performed  at  the  fast  rate,  navigation  axes  calculations  at  the  intermediate  rate,  and 
Earth-related  quantities  are  evaluated  at  the  slow  rate. 

Body  axes  are  Roll,  Pitch,  Yaw;  Navigation  axes  are  the  Geographic  axes  North,  East, 
Down. 

Incremental  gyro  samples  are  8i  and  82  at  the  mid-point  and  end  of  the  fast  calculation 
period.  Incremental  accelerometer  samples  are  A  at  the  end  of  the  period. 

The  operations  are  as  follows: 

Fast  rate:  (body  axes  coordinates) 

Rotation  Vector  6  =  81  -f  82  +  f($i  x  82) 

Attitude  Update  Q  Q  *  {(1  —  .6),  ^8} 

Body  axes  Velocity  V/£  V/£  +  A  —  6  x  (V/£  +  iA) . 
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Intermediate  rate  (navigation  axes  coordinates): 

Earth  rotation  and  transport  Q  ^{(1  —  v).  —  2  v}  *  Q 

Normalise  quaternion  QL  —  1*5  —  0-5  £  ( Q j)2  i  —  0,  3 


Q 

Body  to  nav.  axes 

[v£fr 

coord,  transformation 

then 

v,f 

Total  vel.  increment 

av£ 

Vertical  channel 

Hr 

AK£(D) 

H 


V£(D) 

Horizontal  channels  AX 

V£ 

V£G  for  next  cycle  V£G 

<P  for  next  cycle  q> 


Ql  Q 

=  Q*[VEf]B*Q  l  VefW  V/f 
=  0 

=  +  V£C 

=  H  —  Hb  a  ^  a  Hf 
«-  AK£(Z))  -  (*2  //,  +  *3  at,)  t, 
^H-  { Ve(D )  +  *A  VE{D))t,  -  ki  He 
<-  Ve(D)  +  A  Ve(D) 

*-  AX  -f  (V£  +  i AV£)  /, 

<-v£+ av£ 

—  £  t,  —  (2fi/£  4"  (V£  4-  iz  t,)  t, 


<1 


Slow  rate: 

Update  latitude,  longitude,  sine  and  cosine  of  latitude  from  the  AX;  calculate  gravity  and 
Earth  radius.  Then  AX  =  0. 


5*  DISCUSSION  OF  ITERATION  RATES 

In  the  literature,  the  usual  rate  for  “fast”  calculation  cycles  is  around  100  Hz,  in  the  range 
50  to  200  Hz.  Some  mechanisations  have  a  “slow”  position  cycle  in  the  range  of  about  0  *  5  to  2  Hz. 

Similar  rates  are  envisaged  for  this  algorithm :  the  “intermediate”  rate  calculation  cycle  is 
limited  in  respect  of  its  minimum  rate  as  shown  in  Appendix  10,  by  an  acceleration  error  of 
magnitude  approximately  36/,  parts  per  million  (p.p.m.),  where  t,  is  the  period  of  the  inter¬ 
mediate  cycle.  Thus,  if  say  10  p.p.m.  were  acceptable  for  this  error,  a  higher  limit  of  t,  =  about 
0-25  sec.,  that  is,  a  minimum  frequency  of  about  4  Hz  is  imposed. 

The  other  major  factor  influencing  the  choice  of  intermediate  rate  is  vehicle  speed— a  high 
speed  aircraft,  particularly  if  operating  at  high  latitudes,  would  require  a  higher  iteration  rate, 
whereas  a  helicopter,  for  example,  would  not.  The  attitude  and  body  axis  calculations  have  to 
contend  with  the  full  range  of  vehicle  rates  and  vibration:  their  iteration  rate  should  be  as  high 
as  possible,  unless  the  environment  is  unusually  benign. 

In  practice,  the  capability  of  the  computer  is  a  limiting  factor  on  the  iteration  rate  of  any 
strapdown  system. 

6.  ALGORITHM  PERFORMANCE 

This  will  be  discussed  in  two  parts — attitude  updating,  and  navigation. 

6.1  Attitude  Updating 

Presentation  of  attitude  performance  test  data  will  be  limited  to  an  example  of  coning 
motion.  This  is  a  standard  test  for  evaluating  attitude  algorithms,  as  it  is  a  highly  non- 
commutative  environment,  yet  with  an  analytical  solution. 

The  results  demonstrate  the  effects  of  iteration  and  sampling  rates  and  of  computer  loading; 
see  Figure  2.  Performance  of  the  third  order  algorithm  taking  two  samples  per  iteration  was 
found  to  be  virtually  identical  to  that  of  the  fourth  order  method. 
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6.1.1  Computer  flooding* 

The  third  order  methods  are  almost  identical  in  this  respect.  The  modified  third  order  is 
slightly  more  economical,  requiring  approximately  29  multiplications  and  27  additions  per 
iteration.  The  fourth  order  Runge-Kutta  method  imposes  almost  twice  the  loading  of  the  third 
order  methods.  A  third  order  method  could  therefore  be  run  at  almost  twice  the  rate  of  the 
fourth  order,  and,  as  can  be  seen  from  Figure  2,  this  gives  considerably  more  than  twice  the 
accuracy  in  the  coning  tests. 

6.1.2  Sampling  Rates 

In  order  to  effect  the  non-commutativity  correction,  two  gyro  samples  are  required.  In  this 
algorithm,  as  in  the  Runge-Kutta  method,  two  samples  are  taken  per  iteration.  However,  a 
third  order  Taylor  series  method  often  appears  in  the  literature  (e.g.  Ref.  12),  in  which  only  one 
sample  per  iteration  is  taken.  This  single  sample  method  has  an  accuracy  equivalent  to  that  of 
a  two-sample-per-iteration  method  running  at  half  the  iteration  rate.  In  other  words,  by  taking 
two  samples  per  iteration  instead  of  one,  the  computing  load  may  be  almost  halved. 

This  is  seen  in  the  results,  where  the  single-sample  method  must  run  at  200  Hz  to  give  the 
same  accuracy  as  the  two-sample  methods  running  at  100  Hz. 

6.2  Navigation  Performance 

A  program  (Ref.  13)  which  simulates  the  somewhat  idealised  movement  of  an  aircraft 
above  the  surface  of  an  ellipsoidal,  rotating  Earth,  was  used  to  exercise  the  algorithm.  This 
provided  the  outputs  of  a  strapdown  inertial  measurement  unit  (here  assumed  error-free)  in 
the  aircraft,  which  performed  manoeuvres  including  balanced  horizontal  and  vertical  turns,  with 
prescribed  angular  and  linear  acceleration  and  turning  rates.  The  program  also  outputs  the 
position,  velocity,  and  attitude  of  the  aircraft  during  flight.  The  simulation  is  open  loop;  i.e. 
there  is  no  feedback  of  the  aircraft  state  to  the  flight  control  system,  so  vibration  effects  have 
been  excluded.  However,  the  effects  of  the  dynamic  environment  of  the  I.M.U.  are  primarily 
of  importance  to  the  attitude  segment  of  the  algorithm :  this  exercise  is  considered  to  be  a  valid 
demonstration  of  the  navigation  segment. 

Details  of  the  flight  are  shown  in  Figure  3. 

The  strapdown  algorithm  was  implemented  on  a  27  bit  mantissa  machine :  results  of  two 
runs  are  shown  to  illustrate  the  magnitudes  of  errors  which  arise  from  differing  navigation  cycle 
calculation  rates.  Figure  4  shows  the  result  of  running  all  segments  at  the  fast  rate — 100  Hz. 
Figure  5  was  obtained  with  the  fast,  intermediate,  and  slow  segments  running  at  100  Hz,  10  Hz, 
and  1  Hz  respectively.  For  many  applications,  the  intermediate  and  slow  segments  could  run 
at  even  slower  rates. 
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FIG.  2:  ALGORITHM  DRIFT  FOR  1°  HALF  ANGLE  CONING 


FIG.  3:  SIMULATION  FLIGHT  DETAILS 
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APPENDIX  1 


Notation 


M 

[M]« 


[?1 


Qrb 

F 

R 

V, 

V  IF 

V* 

V*r 

V*c 

AV 

X 

H 

Hb 

vE(D) 

g * 
g 

<4 

5 

6 


a  vector 

vector  M  in  R  frame  coordinates 


the  rate  of  change  of  M  with  respect  to  frame  R 

is  I  —  I  in  £  frame  coordinates.  N.B.  —  j  =  [m]* 

the  transformation  matrix  (direction  cosines)  to  transform  from  R  to  B  coordi¬ 
nates.  i.e.  [MJ*  «  C%[M]* 

the  (vector)  angular  velocity  of  the  £  frame  with  respect  to  the  /  frame 

vector  cross  product  operator 

the  skew  symmetric  matrix  x 

quaternion  “multiplication”  operator 

a  quaternion  representing  a  rotation  from  frame  R  to  frame  B 
specific  force 

position  vector  from  Earth  centre 

velocity  relative  to  inertial  space 

velocity  relative  to  inertial  space  caused  by  specific  force 

velocity  relative  to  Earth 

velocity  relative  to  Earth  caused  by  specific  force 
velocity  relative  to  Earth  caused  by  gravitation 
increment  in  V 
position  vector 
altitude 

barometric  altitude 
vertical  velocity  component 
mass  attraction  (gravitation) 
gravity  (includes  Earth  rotation  effects) 
angular  velocity 


gyro  output 
“rotation  vector” 


ft  =  Jw  dt 


•i 


APPENDIX  2 


Some  Properties  of  Quaternions 


The  uses  of  four  parameter  techniques  to  represent  rotations  are  well  established  in  dynamics. 
Some  properties  of  quaternions  as  applicable  to  this  work  are  listed  below: 

A  quaternion  representing  a  rotation  may  be  expressed  as  a  scalar  and  a  three  element 
vector:  (the  “Euler  Parameters”) 

Q  =  cos  10O,  e\  sin  (|0o),  e2  sin  ($0o),  e$  sin  (i#o). 

This  may  be  interpreted  as  a  rotation  through  an  angle  0o  measured  from  reference  to 
body  axes,  about  a  unit  vector  defined  (in  both  body  and  reference  axes)  by  its  components 
*1,  *2,  *3. 

Alternatively, 

Q  —  cos  (i#o),  (0i/0o)  sin  (£0o),  (#2/0o)  sin  (i^o)*  (Bs/Bq)  sin  (^  0o), 

where  the  are  the  components  of  the  rotation,  and  Bo  =  (0i2  +  022  -r  0a2)1/2-  This  may  be 
written  as  Q  =  C,  65  where  C  =  cos  (i^o)  and  5  =  (l/0o)  sin  (iflo). 

Quaternion  “ Multiplication ” 

Quaternion  “multiplication”  may  be  defined  as  follows:  given  the  quaternions  A  =  Ao,  A 
and  B  =  Bo ,  B,  then  the  product  C  =*  Co,  C,  of  these  is 

C  =  A  *  B  =  (Ao  Bo  —  A.B),  {^4oB  4*  A  Bo  4*  (A  x  B)} . 

For  a  physical  interpretation  of  this,  consider  a  body,  with  a  quaternion  A  representing 
the  rotation  from  reference  to  body  axes.  Give  the  body  a  rotation  such  that  the  quaternion  B 
represents  the  rotation  from  old  to  new  body  axes.  The  quaternion  C  as  defined  above  now 
represents  a  rotation  from  the  reference  axes  to  the  new  body  axes. 

Coordinate  Transformation  by  Quaternions 

The  “unit  length”  rotation  quaternion  Q  =  C,  65  has  a  conjugate  Q*  =  C,  —  65  which  is 

Or'. 

Any  3  dimensional  vector  M  may  be  regarded  as  a  quaternion  fA  with  its  scalar  part  zero : 
then  if  Q  represents  the  rotation  from  reference  to  body  axes, 

[M]R  =  Q  *  [M]B  *  Q-'  s  [M]B} . 


Normalisation  of  Quaternions 

For  a  rotation  quaternion,  the  sum  of  the  squares  of  the  elements  is  nominally  unity.  The 
square  root  of  this  is  sometimes  referred  to  as  the  “length”  of  the  quaternion.  Any  departure 
from  unity  in  this  value  causes  a  “scale  error”. 

This  effect  can  be  removed  by  “normalisation”,  in  which  each  element  of  the  quaternion 
is  divided  by  the  “length”. 

A  “scale  error”  occurs  in  a  vector  whose  coordinates  are  transformed  between  two  coordi¬ 
nate  frames,  where  its  length  does  not  remain  constant. 

A  “drift  error”  occurs  in  a  vector  whose  coordinates  are  transformed  between  two  coordi¬ 
nate  frames,  where  its  direction  does  not  remain  constant. 
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Attitude  Propagation 

A3.1  Direction  Cosines 
For  any  vector  M: 

[M]"  =  CjtMp  (1) 

also, 

[dM/dl]B=CB[til]A  (2) 

Time  derivative  of  (1): 

[m*=c*mA  +  £*[M]A.  o) 

If  <*ab  is  the  angular  velocity  of  frame  B  relative  to  frame  A>  then  Coriolis*  equation,  in 
B  frame  coordinates,  gives : 

[dWt]BA=  [M]s+03e[M]fl.  (4) 

Use  (1H3)  in  (4): 

C2tM]/*  =  cba\m]a  +  CW  + 

i.e.  {CBA  +  nBABCBA}[M\A  =  0 

M  is  any  vector, 

:.cBA  =  -nBABcB. 

Now 

l-n*AB)-1  =  Qj/h  and  n-1  c-1  ee  [CQ]“1, 

/.^-CSQV  (5) 

This  is  the  differential  equation  for  propagation  of  the  matrix  C*. 

A3. 2  Quaternions 
For  any  vector  M: 

[M]fl  =  QA*[M)a*Qab.  (6) 

Differentiate  w.r.t.  time,  and  put  [M]/<  =  QAB  *  [M]B  *  QAB  : 

[M]8  =  Qab  *  Qab  *  [M]8  +  [Mf  *  Qab  *  Qab  +  Qab  *  [M]"  *  QAB  (7) 

where 

Qab  means  djdt  (QA^) . 

Now 

Qab  *  lM]A  *  =  Cj  [M]^. 

Using  (2)  and  (4),  we  get 


Qa'b  *  M4  *  ft»  =  [M]8  +  n%mB. 


(8) 


Consider  the  identity  q~l  *q  =  1,  where  q  is  any  rotation  quaternion: 

q  =  qo,  q  and  q  l  =  q0,  -q. 

Differentiate  w.r.t.  time: 


q~l  *  q  +  q~l  *  q  =  0 . 


Now,  by  definition 

4"l*9={4o9o  +  q*q},Woq-q9o-(q  x  q)}  == />o,  p  (say) 


and, 

tf'1  *4  =*  {4oqo  +  q  q}»  {qo  q  -  q4o  ~  (Q  x  q)}  =  r0,  r  (say) . 
It  can  be  seen  that  po  =  ro  and  p  =  — r 
however,  from  (9):  po,  p  +  ro,  r  =  0  therefore  po  —  ro  =  0 . 


W 


In  equation  (7),  let  0^  *  =  P  and  QA'B  *  =  ~P 

Using  (8),  (7)  may  then  be  written 

[M]*  =  P  *  [M]8  -  [M]8  *  p  +  [M]8  +  n*B  [M]8 . 
Evaluating  the  quaternion  products,  and  writing  [<j,,b]b  x  for  Q8B, 

/.  0  =  -p[M]8  +  p  x  [M]8  +  p.[M]8  -  [M]8  x  p  +  K.J*  x  [M]8 
{2p  +  KJ5}  x  [M]8  =  0. 


M  is  any  vector. 


—  — P  —  Qab  *  Qab 
Qab  =  \Qab  * 


(10) 


Alternatively,  if  [<• >AB]A  is  available,  (10)  becomes  QAB  —  *  QAB 


(10)  is  the  differential  equation  for  the  propagation  of  quaternion  QAB. 
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Quaternion  Update  Methods 

A4. 1  Taylor  Series  Expansion  ( to  Third  Order ) 

We  wish  to  use  the  equation 

&O=»0(O*SV)  0) 

to  obtain 

Q(t  +  h). 

Using  a  Taylor  Series  expansion,  we  get: 

Q(t  +  h)  =  <5(0  4-  *5(0  +  (h2/2l)  Q{t)  +  (/r3/3 !)  £(0  +  . . .  (2) 

[for  clarity,  the  (/)  will  be  omitted]. 

Differentiate  (1)  w.r.t.  time: 

Q  —  *  <o)  *  w  +  iQ  *  w. 

Now, 

(Q  +  to)  +  uj  =  Q  +  (oj  +  a>)  =  5(~ww) 

.'.0  =  0*  {h&  -  w)}  ■  (3) 

Differentiate  again : 

Q  —  idQ  *  s  4-  45  *  ®  —  Kw‘w)0  —  Kw'w)(}5  *  “)- 

Now, 

(^  *  ai)  ♦  o»  =  ^  *  (w  ♦  cu) 

£?  —  5  *  {i(<*>  *  <0  —  i(w*w)  “  §(ww)«  4-  iw) .  (4) 

Using  (1),  (3),  and  (4)  in  (2),  we  get 

Q(t  +  h)=  Q{t)  *  (7(0  (5) 

where : 

(7=14-  \ho)  4-  \tiL  {Jcu  —  J(u*  •  w)}  4-  lh3  (i(a>  *  cu)  —  J(a)'Cu)  —  i(u,'u>)  4-  itu  •}  (6) 

Expand  0  into  its  scalar  and  vector  components:  [N.B.  (a»  *  ch)  =  — 4-  (w  x  w).] 

Uot  U  =  {1  —  —  J/*3(w  w)},  {§Au>  4-  \h26i  4-  ^Vf3(w  x  w)  —  ? V^3(u>  w)u>  +  iV*3w}.  (7) 

The  values  of  w(/),  «*>(/)  and  w(0  must  now  be  found  from  the  gyro  outputs.  It  is  assumed  that 
a  second  order  polynomial  may  be  fitted  to  the  gyro  outputs:  i.e.,  8(0  =  4-  b/2.  Now, 

8(0  =  jv>(t)dt  over  the  sample  period:  this  implies  that  u>(0  =  0,  and  <*>(0  *s  constant  over 
the  period. 

If  the  gyros  are  sampled  at  the  mid-point  and  end  of  each  iteration  period,  giving  8i 
from  t  to  (/  +  and  82  from  ( t  4-  \h)  to  (/  4-  h\  we  get  u>(0  =  (38i  —  8 2)lh  and 

«(0  =4(82-8  0/A* 


Substituting  in  (7),  rearranging,  and  putting  8  =  (81  +  82),  we  get 
t/o  =  1  —  K8-8)  +  i(*i  -  «2)  (8i  -  82) 

U  =  *6  +  J(»l  X  82)  -  j'j  {(8.8)  -  881  (62  -  8,)}  {8  -  2(62  -  8,)} . 

The  last  term  in  each  part  of  0  may  be  assumed  small:  if  in  these  terms  only,  we  put  81  =  62, 
then  we  get 

^o=l—  4(6.6) 

11  =  46  +  J($i  X  62)  -  A  (6.6)6. 


It  is  interesting  to  compare  this  result  with  the  solution  obtained  from  the  rotation  vector 
method  using  a  third  order  expansion  of  C  and  5:  in  that  case; 

C,  56  =  (l  —  £0o2)>(je  -  *0O2  6) 

where 


6  =  61  +  52  +  §(61  x  62)  and  0O2  =  6.6. 
If  (81  x  82)2  and  #o2(6i  x  82),  which  are  small,  are  neglected,  we  get 

C  —  \  —  J(6.6) 

56  -  J6  +  4(6X  X  62)  -  -4i^6.6)  6 
i.e.  U0,  U  =  C,  56. 


A  method  of  estimating  c *>(/)  and  o i(f)  which  often  appears  in  the  literature  (e.g.  Ref.  12) 
uses  the  incremental  gyro  output  8  over  the  whole  calculation  period,  that  is,  between  time  t 
and  time  (/  -f  /?),  and  the  previous  output  6',  between  times  (/  —  h)  and  t. 

Using  these  values,  we  get  co(f)  =  (6  -f  8  )  2/z  and  <*>(/)  =  (8  —  $')/h2  . 

When  these  are  substituted  into  (7),  we  get,  after  rearrangement, 

Uo  =  1  -  4(6.6)  +  3^(6  -  8'). (8  -  8') 

U  =  46  +  *(8'x  8)  -  *(6.6  -  4(38  +  6').(8  -  6')}(8  +  6'). 

The  last  term  in  each  part  of  0  may  be  assumed  small:  in  these  terms,  if  one  assumes  that 
8  =  8',  the  result  is  obtained : 

Uo  =  1  -  4(6-6) 

U  =  i*  +  *(6'x6)-*(8.6)6. 


A4.2  Quaternion  Update  by  Fourth  Order  Runge-Kutta 

Runge-Kutta  methods  are  standard  tools  in  numerical  analysis.  The  application  of  a  fourth 
order  method  to  the  attitude  quaternion  update  is  as  follows: 

Given  the  equation  Q(t)  =  lQ(t)  *  £>(/):  to  find  Q(t  +  h). 

Incremental  gyro  outputs  8i  and  82  are  taken  at  the  mid-point  (time  t  +  \h)  and  end 
(time  t  +  h)  of  the  period  respectively.  Fitting  a  second  order  polynomial  to  these  allows 
calculation  of  the  angular  velocity  at  the  start,  mid-point,  and  end  of  the  period : 

u>(/)  =  (38x  —  82)/7j,  c*)(/  -f-  \h)  =  (8i  +  8 2)jhy  u>(/  +  h)  =  (362  —  h\)jh 


Let 


Hu>o  =  h.i*(t) 

=  (38 1  -  82) 


and 


Ho>i  =  h.iMj  +  \h)  and  Hu>2  =  ha(t  +  h) 
=  (6 1  +  62)  =  (38i  —  5i) . 


The  method  is: 


1)  k0  =  hQ(t)  =\Q(t)*Hwo 

2)  Q'(t  +  i h)  -  5(0  +  J/co 

ki  =  hQ\t  4-  JA)  =  iQ'Q  +  4A)  *  Hoji 

3)  5"('  +  4/1)  =  5(0  +  4*1 

Ft  =  hQ‘\t  +  4 h)  =  45"('  +  ih) .  HFl 

4)  5'('  +  A)  =  5(0  +  F2 

ki I  =  hQ\t  +  h)  =  45'(f  + 

5)  update:  Q{t  +  h)  =  5(0  +  4(*i  +  2  (*T  +  ib)  +  Fz) . 
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Rotation  Vector  Equation 


= 


Consider  a  quaternion  Q  =  C,  S8:  C  =  cos  (i#o),  S  =  (1/0 o)  sin  (|0o)*  #o  —  (O.0)1'2,  • 
is  the  rotation  vector. 

,  1(0.0  +  e.e)  e.e 

Differentiate:  4  — 

also 
and 
and 

therefore 
Put 


(e.e)1^  0O 

C  =  -sin(i0o).R=  -i5(le) 

0  1  (b  B) 

5  =  -  ft  sin  (i«o)  +  J  COS  (*fl») .  (C  -  2S) 


5  =  C,  (5«  +  SO) 

Q  =  }{ -5(6.6).  ^  (C  -  25)  0  +  256} . 

(6.6)6  =  6x(8x6)-fW 

^  ,  (C  —  2S) 


Q  =  }{— 5(8.8),  ce  + 


-  6  X  (6  X  6)} 


(0 


also: 

Now,  (C,  56)  =  (Go,  Q), 


Q  =  \Q  * "  =  ${-(Q-°»).  Co«*»  +  (Q  x  <*»)}• 


Q  =  ${— 5(8.o>),  Coj  +  5(6  x  u»)}. 


(2) 


Equating  scalar  parts  of  Q  in  (1)  and  (2),  we  get 

0.u>  =  0.8 

i.e.  the  component  of  0  parallel  to  0  is  equal  to  the  component  of  w  parallel  to  0. 


(3) 


Equating  vector  parts  of  Q  in  (1)  and  (2),  we  get 

(C  -  25) 


C0  + 


0  x  (0  X  0)  =  Cw  +  5(0  X  w) . 


Consider  the  relations 
and 


0  X  (0  X  0)  ■  (0.0)  0  -  (0.6)0 


0  X  (0  X  w)  =  (0.u>)0  —  (0.8)  o> . 

Subtracting  these,  and  using  (3),  we  get 

0  x  (0  x  0)  =  0  x  (0  x  w)  —  0o2(0  —  w)  • 

Substitute  in  (4): 

ce  +  -C-^-2-- 6  x  (8  x  w)  —  (C  —  25K8.W)  =  Cu>  +  5(8  x  w). 
0<r 

Simplifying,  we  get 

8  =  «  +  j(6xu)  +  (W)  (I  -  (C/25)]  6  X  (6  X  «). 


(4) 


(5) 


Making  the  approximations  C  =  1  —  i#o2  and  5  =  i(*  —  we  8et 


4  =  w  +  J(8  x  u)  +  -fop  x  (8  x  w) . 
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Solution  for  the  Rotation  Vector 


For  present  purposes,  the  very  small  triple  vector  product  term  will  be  neglected,  thus: 

*0  =  **0  +  i{e(/)xa>(0}.  0) 

Assume  second  order  gyro  output  variation: 

6(0  =  a  t  +  hr*  (2) 

where  T  <  t  <  T  +  h  :  T  is  time  at  the  start  of  the  period  of  length  h,  Therefore  w(f)  =  a  +  2bf, 
«(/)  =  2b,  and  =  0. 

Now,  6(7)  =  0,  u>(T)  =  a,  w(7)  =  2b. 
equation  (1)  gives  6(7)  —  o >(7)  =  a 

differentiate  and  substitute:  6(7)  =  a>(7)  =  2b 

differentiate  and  substitute:  6(7)  =  i«*K7)  x  u>(7)  ^axb 

and  e (7)  is  a  triple  vector  product,  which  is  neglected. 


Apply  a  Taylor  Series  solution  to  equation  (1): 

6(7  4-  h)  -  6(7)  +  A6(7)  +  (1/2!) ^6(7)  +  (1/3!)  h**(T)  +  . . . 

therefore 

6(7  +  h)  =  %h  +  bA2  4-  i/*3(a  x  b). 


(3) 


If  &i  is  the  incremental  gyro  output  at  the  mid  point  of  the  period,  and  62  is  that  at  the  end, 
then  (eq.  2) 

6(7+ iA)  =  *i  =  *•*  +  *«** 

and 

6(7  +  h)  =  6^  +  62  =  lA  b h? . 

Solving  for  a  and  b,  we  get 


a h  =  38j  —  62  and  b h2  =  2(62  —  $1) . 


Therefore 

x  b)=  Ksi  x  62) 

and  equation  (3)  may  be  written 

6(7+  h)  =  81  +  52+  1(6!  x  62). 

The  same  procedure  may  be  used  for  higher  order  solutions  if  necessary. 


1 
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Derivation  of  the  Navigation  Equation 


Apply  Coriolis*  equation  to  R: 

(?), =  (*)£ + n,cR =  v* + a,E*' 

Differentiate 

'•  (S), =  (ir), +  Q,eVe  +  a,E  Q,E  R ' 

Specific  force  is  given  by  F  =  (, d2R/dt2)l  —  gm  where  gm  is  mass  attraction 

(5),  =  F  +  <*»  -  Q>£a*  R)  -  v£. 

Gravity  is  defined  as  the  resultant  of  mass  attraction  and  centrifugal  force:  i.e. 

8  =  8 m  Q[e  &ie  R 

For  any  frame  K ,  Coriolis’  equation  applied  to  V£  gives 

Put  Qjk  =  Qie  +  and  substitute  for  ( d\E/dt)j : 

=  F  +  g  -  (2Q„  +  £!„)  V£. 
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SpUt-Fnune  Mechanisation 


The  concept  applied  here  is  that  changes  in  position  caused  by  specific  force  (R£)  and 
those  caused  by  gravitation  (Rc)  may  be  calculated  separately.  The  actual  change  in  position 
(R)  is  given  by  R  =  RF  +  R<;. 

Specific  force  is  given  by  F  =  (d?Rjdt2)i  —  g„;  g«  is  mass  attraction  vector 


then 


The  initial  conditions  are  R£=0;  (dRFldt)t=  V/F=  0;  (dRF/dt)£  =  V£F=  0,  and  RC==R; 
C dRoldt ),  =  VK  =  V,;  (dRJdt)B  =  \EQ  =  V£. 


A8.1  The  Specific  Force  Component 
Coriolis*  equation  applied  to  \!F: 

but 

w-w-’ 

(^),=F_n,flv'F- 

When  this  is  expressed  in  body  frame  coordinates,  it  may  be  written : 

[V/f]*  =  [F]*  -  Of£tV/f]B. 

This  is  the  equation  for  the  velocity  component  caused  by  specific  force. 


A8.2  The  Gravitation  Component 


Coriolis*  equation  applied  to  Rc: 


d*o\  _  (d*o\ 

,dt  )t  \dt)s 


+  R^ 


i.e. 


V/g  —  Vgc  +  D/£Rc. 


Differentiate: 


Therefore 


r 


m.-m-'&k 


(dVja] 
\  * 


'-Km  —  ^  +  ^lE^EG  +  ^/£^/£^C* 

Coriolis*  equation  applied  to  for  any  frame  K : 

Substitute  for  {d\BGldt),\ 

Zm  =  +  (^a  +  ^£C  +  ^i£  ^/£  • 

Rf  is  small  compared  with  the  distance  to  Earth  centre  R  =  Rc  *+*  Rf,  so  we  may  put 

E  “  E*i  “  ^#£  **/£ 

also 

QIX  =  Qie  -f  H£jr 

therefore 


(*H-- 


(2D/£  +  flfjf)  V eg- 


This  is  the  equation  for  the  velocity  component  caused  by  the  effect  of  gravitation. 


APPENDIX  9 


Solution  of  Body  Axis  Velocity  Equation 

We  wish  to  use  the  equation  (Appendix  8) 

V(0  =  F(/)  -  0(0  V(0  (1) 

to  obtain  V(/  +  A). 

Using  a  Taylor  Series  expansion,  we  get  [omitting  the  (/)]: 

\(t  +  A)  =  V  +  h\  +  (A*/2!)  V  + -  (2) 

Differentiate  (1): 

V  —  F  —  ClV  —  D(F  —  DV) .  (3) 

Using  (1)  and  (3)  in  (2): 

V(f  +  A)  =  V  +  h¥  -  ADV  +  (A2/2)(F  -  DV  -  DF  +  QDV). 

Assuming  that  specific  force  and  angular  velocity  are  constant  during  the  interval,  and  that 
the  term  (A2/2)  OQV  may  be  neglected,  we  get 

V(f  +  A)  =  V  +  AF  -  AD  (V  +  £AF). 

Now,  AF  is  the  accelerometer  output  A,  and  AD  is  the  skew  symmetric  of  the  rotation  vector, 
i.e.  AD  =  [8  v  ]. 
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Relationship  between  VlF  and  VEf 


Coriolis*  equation  applied  to  RF  gives: 


d*f\  =  (d*f\ 
,dt)J  \dt)E 


+  ^/£  R/r 


or,  in  terms  of  velocity,  over  an  interval  h; 

Vjf  —  V ef  +  &ie  f  V/fdt 
J  0 

(at  the  start  of  the  interval,  V/F  =  \EF  =  0). 


If  constant  specific  force  F  is  assumed  through  the  interval 

CvlFdt  =  ivJF.h 

J  o 

therefore 

^EF  =  0  V IF' 


The  \ef  error  per  interval  caused  by  assuming  that  VEi?  =  V/jp  is  i A 
this  is  equivalent  to  a  specific  force  error  FA,  i.e.  approx.  36h  parts  per  million. 
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Rotation  of  Navigation  Axes  (relative  to  Inertial) 


Body  axis  rotation  obtained  via  the  gyros  is  relative  to  inertial  space.  The  attitude  quaternion 
stored  in  the  computer  relates  body  axes  to  navigation  axes.  Therefore,  between  navigation 
updates,  the  navigation  axes  rotation  must  be  allowed  for. 


In  the  diagram,  N1  and  B),  and  N2  and  B2 
respectively  represent  the  navigation  and  body 
axes  at  times  FI  and  72.  Rotations  are  relative 
to  inertial  space. 


N1  M2 


B1 

B2 


For  time  71,  the  quaternion  of  rotation  from  N1  to  B1  is  in  the  computer  (Ql). 

Between  71  and  72,  the  quaternion  of  rotation  from  B1  to  B2  is  calculated  from  the  gyro 
outputs  (QG),  and  the  attitude  quaternion  relative  to  the  N1  axes  is  updated: 

Qla  =  Ql  •  QG 

(Qla  is  in  N1  and  B2  coordinates,  and  represents  the  rotation  from  N1  to  B2). 

The  rotation  from  N1  to  N2  during  this  period  is  calculated,  based  on  the  conditions  at  71. 
This  is  represented  by  QN  (in  N1  and  N2  coordinates). 

For  time  72,  the  quaternion  Q2  representing  the  rotation  N2  to  B2  is  calculated  as  follows: 

the  rotation  B2  to  N1  is  represented  by  Qla-1 
the  rotation  N1  to  N2  is  represented  by  QN 

therefore  the  rotation  B2  to  N2  is  represented  by  Qla-1  *  QN  (in  B2  and  N2  coordinates) 
therefore 

Q2  =  [Qla-1  ♦  QN]1  =  QN-1  *  Qla 
This  is  the  required  attitude  quaternion  at  72. 


i 
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Transformation  by  Quaternion 


For  any  vector  V : 

[V]c  =  Q  *  [V]**  Q  l  (1) 

where  Q  is  the  quaternion  representing  coordinate  rotation  from  the  G  to  the  B  axes. 

Write  0  =  (0o,  Q).  Q~l  —  (Qoy  —  Q),  and  evaluate  (1)  using  the  rules  for  quaternion 
multiplication : 
this  gives 

\V]G  =  C?o2[V]B  +  (Q . [V]B)Q  +  2Q<*Q  x  [V]*)  +  Qx(Qx  [V]B). 

Now, 

(Q  •  (V]B)Q  =  Q  x  (Q  x  [V]*)  +  (Q  Q)  [V]*,  also  (0O2  +  Q .  Q)  =  1 

therefore 

[\]G  „  [V]B  +  2Q0(Q  x  m  +  2Qx(Qx  [V]B)  (2) 


Writing  the  elements  of  [V]B  as  VBi,  VB2,  VB3,  and  those  of  Q  as  Qi,  Q2,  Qs,  (2)  may  be 
written  in  full: 

VBX  +  2KMQ2.VB3  -  Q3.VB2)  +  Q2(Qi.VB2  -  Q2.VB1)~Q3(Q3.VBi  -  Q1.VB3)] 
VB2  +  2[Qo(Q3. VBi  -  Q1.VB3)  +  Q3(Q2  VB3  -  Q3  VB2)  -  Qi(Qi.VB2  -  Q2  VB1)] 
VBS  +  2[Qo(Qi.  VB2  -  Q2.VB1)  +  Qi(Qj.  VBi  -  Qi.  VBS)  -  Q2(Q2.  VB3  -  Qs.  VB2)]  J 
this  is  the  quaternion  transformation. 


[Vc]  = 


If  we  collect  the  VB’s  together  and  take  them  outside,  we  get  the  Direction  Cosine  Matrix 
in  terms  of  the  quaternion: 


-  1  -  2(Q22  +  Qs2)  2(Qi.Q2  -  Q0.Q3)  2(Qo.Q2  +  Qi.Qs)  ' 

VBr 

fV]c  = 

2(Qo.Qs  +  Q1.Q2)  1  -  2(Qi2  +  Qs2)  2(Q2.Qs  -  Qo-Qi) 

vb2 

.  2(Qi.Q3  -  Q0.Q2)  2(Q0.Qi  +  Q2.Qs)  1  -  2(Qi2  +  Qt2)  . 

_VBs. 

i.e.  [V]c  =  C®[V]B. 


APPENDIX  13 


Slow  Cycle  Operations 


AI3.1  Update  of  Position 

Position  is  maintained  as  latitude  and  longitude  angles.  At  the  start  of  each  slow  cycle, 
new  values  XN  and  XE  of  incremental  (since  the  t  revious  slow  cycle)  distance  travelled  in  north 
and  east  directions  are  available. 

The  change  in  latitude  is  given  by  XNjRN ;  in  longitude  by  XEjRE  cos  (A). 


A13.2  Update  of  Sine  and  Cosine  of  Latitude  (L) 

The  angular  change  (A)  in  latitude  is  A  =  XN/RN. 
Now, 


then 


sin  (L  +  A)  =  sin  ( L )  cos  (A)  +  cos  (L)  sin  (A),  or,  if  (A)  is  small, 


similarly 


sin  (L  +  A)  =  sin  (L)  +  A  cos  ( L ) 
cos  (L  +  A)  =  cos  (L)  —  A  sin  (L) . 


For  a  vehicle  travelling  at  approximately  640  m/s,  and  a  slow  cycle  period  of  one  second,  this 
angle  is  approximately  0  0001  radian,  and  the  errors  in  sin  (JL  +  A)  and  cos  (L  +  A)  would  be 
less  than  0  01  part  per  million  per  second.  If  this  were  unacceptable,  then  a  faster  slow  cycle 
rate  would  be  used. 


A13.3  Update  of  Earth  Constants 

The  shape  of  the  Earth  may  be  approximated  to  an  ellipsoid  of  revolution.  This  approxi¬ 
mation  is  widely  used  to  obtain  formulae  for  calculation  of  gravity  and  local  radius  of  Earth. 

Formulae  for  the  vertical  component  of  gravity  g,  and  the  north  RN ,  and  east  RE,  values 
of  the  Earth  radius  of  curvature  are  given  in  Reference  1 1 : 

g  =  g0  (1  +  0  0052884  S'  —  3*157  x  10"6  H  +  smaller  terms) 

RN  —  R  (l  -2e  +  3 e  S) 

RE  =  R(l+eS) 

where  S  is  sin  (latitude),  H  is  altitude  (metres),  go  =  9*78049  m/s2,  /?  =  6378160  m,  and 
e  =  1/298*25, 

these  may  be  written 

g  =  9*78049  +  0  051723  5  —  3*088  x  10  «  H 
RN=  6335389  +  64155*84  S 
RE  =  6378160  +  21385*28  5'. 
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Vertical  Channel  Damping 

A14.1  Pure  Inertial  (Undamped)  System 

The  vertical  component  of  gravity  is  calculated  from  g  =  go(l  +  A  sin 2  L  —  2 H/R)  where 
H  is  altitude,  L  is  latitude,  R  —  6378160,  A  —  0-05288,  and  go  =  9  *78049. 

For  an  error  H  in  estimated  altitude,  there  is  an  error  in  g  given  by 

g  =  -H  2gojR . 

The  vertical  channel  equations  are  H  =  v  and  v  =  /  —  g  +  Coriolis  terms. 

The  vertical  channel  error  equations  are  H  =  v  and  v  =  — g 
therefore 

11=  k  =  H  .  2go/R . 

The  solution  of  this  has  the  form  H  =  A  exp[V2go/R  /]  +  £exp[— VlgojR  t). 

The  time  constant  of  the  instability  is  therefore  VR/2go  =  approx.  570  sec. 

A! 4.2  Baro~ Inertial  Third  Order  System 

Given  a  measurement  of  the  barometric  altitude  Hb,  the  vertical  channel  equations  are 
modified  to  : 

//=  v-  KHH-Hb)  (1) 

v  =  /  —  g  —  K2  (H  —  Hb)  —  a  +  Coriolis  terms  (2) 

where 

a  =  A3  (H  -  Hb)  (3) 

Differentiate  (2),  substitute  for  a ,  then  write  as  error  equations  : 

H  =  v-K\{H-Hb)  (4) 

v  -  H  .  2go/R  -  A2  (//  -  Fib)  -  A3  (H  -  Hb)  .  (5) 

Differentiate  (4)  twice,  and  substitute  for  v: 

fl  +  AJ  ft  +  (A2  -  2go!R)  H  +  A3  H  =  A1  +  A2  /f*  +  A3  £b.  (6) 

Following  Reference  11,  the  characteristic  equation  is  of  the  form 

(s  +  l/r)3//  -  0 

this  is  satisfied  if  A1  =  3/r,  A2  =  3/r*  4-  2g0/A,  A3  =  l/r». 

So,  for  r  =  100  seconds,  A!  =  0*03  s-i,  A2  =  3  03  x  10^  A3  =  I0'« 
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